#include "../include/Optimizer.hpp"
G2O_USE_OPTIMIZATION_LIBRARY(eigen);
G2O_USE_OPTIMIZATION_LIBRARY(dense);
namespace ORBSLAM
{
    Optimizer::Optimizer(/* args */)
    {
    }

    Optimizer::~Optimizer()
    {
    }

    void Optimizer::Global_bundle_adjustment(Map *p_map, int iterations, bool *pb_stopflag, const unsigned long n_loop_KF, const bool b_robust)
    {
        vector<KeyFrame *> vp_KF = p_map->Get_all_keyframe();
        vector<MapPoint *> vp_mpt = p_map->Get_all_mappoint();
        Bundle_adjustment(vp_KF, vp_mpt, iterations, pb_stopflag, n_loop_KF, b_robust);
    }

    void Optimizer::Bundle_adjustment(const vector<KeyFrame *> &vp_KF, const vector<MapPoint *> &vp_mpt, int iterations, bool *pb_stopflag, const unsigned long n_loop_KF, const bool b_robust)
    {
        vector<bool> vb_not_included_mpt;
        vb_not_included_mpt.resize(vp_mpt.size());

        Map *p_map = vp_KF[0]->Get_map();

        g2o::SparseOptimizer optimizer;
        string solver_name = "lm_fix6_3";
        g2o::OptimizationAlgorithmProperty solverProperty;
        optimizer.setAlgorithm(
            g2o::OptimizationAlgorithmFactory::instance()->construct(solver_name,
                                                                     solverProperty));

        if (pb_stopflag)
        {
            optimizer.setForceStopFlag(pb_stopflag);
        }

        long unsigned int max_KFid = 0;
        const int n_reserve_size = (vp_KF.size() * vp_mpt.size());
        vector<g2o::EdgeSE3ProjectXYZ *> vp_edge_mono;
        vp_edge_mono.reserve(n_reserve_size);
        vector<KeyFrame *> vp_edgeKF_mono;
        vp_edgeKF_mono.reserve(n_reserve_size);
        vector<MapPoint *> vp_edgepoint_mono;
        vp_edgepoint_mono.reserve(n_reserve_size);

        for (int i = 0; i < vp_KF.size(); i++)
        {
            KeyFrame *p_KF = vp_KF[i];
            if (p_KF->Is_bad())
            {
                continue;
            }
            g2o::VertexSE3Expmap *vSE3 = new g2o::VertexSE3Expmap();
            vSE3->setEstimate(Converter::toSE3Quat(p_KF->Get_Tcw()));
            vSE3->setId(p_KF->mn_keyframe_id);
            vSE3->setFixed(p_KF->mn_keyframe_id == p_map->Get_init_KFid());
            optimizer.addVertex(vSE3);
            if (p_KF->mn_keyframe_id > max_KFid)
            {
                max_KFid = p_KF->mn_keyframe_id;
            }
        }
        const float th2d = sqrt(5.99);
        const float th3d = sqrt(7.815);
        for (int i = 0; i < vp_mpt.size(); i++)
        {
            MapPoint *p_mpt = vp_mpt[i];
            if (p_mpt->Is_bad())
            {
                continue;
            }
            g2o::VertexPointXYZ *pPt = new g2o::VertexPointXYZ();
            pPt->setEstimate(Converter::toVector3d(p_mpt->Get_world_pose()));
            const int id = p_mpt->mn_id + max_KFid + 1;
            pPt->setId(id);
            pPt->setMarginalized(true);
            optimizer.addVertex(pPt);

            const map<KeyFrame *, int> observations = p_mpt->Get_observation();
            int nedge = 0;
            for (map<KeyFrame *, int>::const_iterator mit = observations.begin(); mit != observations.end(); mit++)
            {
                KeyFrame *pKF = mit->first;
                if (pKF->Is_bad() || pKF->mn_keyframe_id > max_KFid)
                {
                    continue;
                }
                if (optimizer.vertex(id) == NULL || optimizer.vertex(pKF->mn_keyframe_id) == NULL)
                {
                    continue;
                }
                nedge++;
                const int idx = mit->second;
                const KeyPoint &kpun = pKF->mv_orb_unkeypoints[idx];
                Eigen::Matrix<double, 2, 1> obs;
                obs << kpun.pt.x, kpun.pt.y;
                g2o::EdgeSE3ProjectXYZ *e = new g2o::EdgeSE3ProjectXYZ();
                e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(id)));
                e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(pKF->mn_keyframe_id)));
                e->setMeasurement(obs);
                const float &invsigma2 = pKF->mvd_inv_level_sigma2[kpun.octave];
                e->setInformation(Eigen::Matrix2d::Identity() * invsigma2);
                if (b_robust)
                {
                    g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;
                    e->setRobustKernel(rk);
                    rk->setDelta(th2d);
                }
                // TODO
                e->fx = 458.654;
                e->fy = 457.296;
                e->cx = 367.215;
                e->cy = 248.375;

                optimizer.addEdge(e);
                vp_edge_mono.push_back(e);
                vp_edgeKF_mono.push_back(pKF);
                vp_edgepoint_mono.push_back(p_mpt);
            }

            if (nedge == 0)
            {
                optimizer.removeVertex(pPt);
                vb_not_included_mpt[i] = true;
            }
            else
            {
                vb_not_included_mpt[i] = false;
            }
        }

        optimizer.setVerbose(true);
        optimizer.initializeOptimization();
        optimizer.optimize(iterations);
        cout << "BA: End of the optimization" << endl;

        for (int i = 0; i < vp_KF.size(); i++)
        {
            KeyFrame *pKF = vp_KF[i];
            if (pKF->Is_bad())
            {
                continue;
            }
            g2o::VertexSE3Expmap *vSE3 = static_cast<g2o::VertexSE3Expmap *>(optimizer.vertex(pKF->mn_keyframe_id));
            g2o::SE3Quat SE3quat = vSE3->estimate();
            if (n_loop_KF == p_map->Get_orignKF()->mn_keyframe_id)
            {
                pKF->Set_pose(Converter::toCvMat(SE3quat));
            }
            else
            {
                pKF->mm_Tcw_gba.create(4, 4, CV_32F);
                Converter::toCvMat(SE3quat).copyTo(pKF->mm_Tcw_gba);
                pKF->mn_loopKF_gba = n_loop_KF;
            }
        }

        for (int i = 0; i < vp_mpt.size(); i++)
        {
            if (vb_not_included_mpt[i])
            {
                continue;
            }
            MapPoint *p_mpt = vp_mpt[i];
            if (p_mpt->Is_bad())
            {
                continue;
            }
            g2o::VertexPointXYZ *vpt = static_cast<g2o::VertexPointXYZ *>(optimizer.vertex(max_KFid + 1 + p_mpt->mn_id));
            if (n_loop_KF == p_map->Get_orignKF()->mn_keyframe_id)
            {
                p_mpt->Set_world_pose(Converter::toCvMat(vpt->estimate()));
                p_mpt->Update_normal_and_depth();
            }
            else
            {
                p_mpt->mm_world_pose_gba.create(3, 1, CV_32F);
                Converter::toCvMat(vpt->estimate()).copyTo(p_mpt->mm_world_pose_gba);
                p_mpt->mn_loopKF_gba = n_loop_KF;
            }
        }
    }

    int Optimizer::Pose_optimization(Frame *p_frame)
    {
        g2o::SparseOptimizer optimizer;
        string solver_name = "lm_fix6_3";
        g2o::OptimizationAlgorithmProperty solverProperty;
        optimizer.setAlgorithm(
            g2o::OptimizationAlgorithmFactory::instance()->construct(solver_name,
                                                                     solverProperty));
        optimizer.setVerbose(false);

        int n_initial_correspondences = 0;
        g2o::VertexSE3Expmap *p_SE3_vertex = new g2o::VertexSE3Expmap();
        p_SE3_vertex->setEstimate(Converter::toSE3Quat(p_frame->mm_Tcw));
        p_SE3_vertex->setId(0);
        p_SE3_vertex->setFixed(false);
        optimizer.addVertex(p_SE3_vertex);

        const int n = p_frame->mn_feature_num;
        vector<g2o::EdgeSE3ProjectXYZOnlyPose *> vp_edges;
        vector<int> v_edge_idx;
        vp_edges.reserve(n);
        v_edge_idx.reserve(n);

        const float th2 = sqrt(5.991);

        for (int i = 0; i < n; i++)
        {
            MapPoint *p_mpt = p_frame->mvp_mappoints[i];
            if (p_mpt)
            {
                n_initial_correspondences++;
                p_frame->mvb_outline[i] = false;
                Eigen::Matrix<double, 2, 1> obs;
                const KeyPoint &unkp = p_frame->mv_orb_unkeypoints[i];
                obs << unkp.pt.x, unkp.pt.y;
                g2o::EdgeSE3ProjectXYZOnlyPose *e = new g2o::EdgeSE3ProjectXYZOnlyPose();
                e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(0)));
                e->setMeasurement(obs);
                const float inv_sigma2 = p_frame->mvd_inv_level_sigma2[unkp.octave];
                e->setInformation(Eigen::Matrix2d::Identity() * inv_sigma2);
                g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;
                e->setRobustKernel(rk);
                rk->setDelta(th2);

                e->fx = 458.654;
                e->fy = 457.296;
                e->cx = 367.215;
                e->cy = 248.375;

                Mat Xw = p_mpt->Get_world_pose();
                e->Xw[0] = Xw.at<float>(0);
                e->Xw[1] = Xw.at<float>(1);
                e->Xw[2] = Xw.at<float>(2);

                optimizer.addEdge(e);
                vp_edges.push_back(e);
                v_edge_idx.push_back(i);
            }
        }
        if (n_initial_correspondences < 3)
        {
            return 0;
        }
        const float chi2_array[4] = {5.991, 5.991, 5.991, 5.991};
        const int its[4] = {10, 10, 10, 10};
        int nbad = 0;

        for (int it = 0; it < 4; it++)
        {
            p_SE3_vertex->setEstimate(Converter::toSE3Quat(p_frame->mm_Tcw));
            optimizer.initializeOptimization(0);
            optimizer.optimize(its[it]);

            nbad = 0;
            for (int i = 0; i < vp_edges.size(); i++)
            {
                g2o::EdgeSE3ProjectXYZOnlyPose *e = vp_edges[i];
                const int idx = v_edge_idx[i];

                if (p_frame->mvb_outline[idx])
                {
                    e->computeError();
                }

                const float chi2 = e->chi2();
                if (chi2 > chi2_array[it])
                {
                    p_frame->mvb_outline[idx] = true;
                    e->setLevel(1);
                    nbad++;
                }
                else
                {
                    p_frame->mvb_outline[idx] = false;
                    e->setLevel(0);
                }
                if (it == 2)
                {
                    e->setRobustKernel(0);
                }
            }
        }
        // cout << "nbad:" << nbad << endl;
        g2o::VertexSE3Expmap *p_SE3_recov = static_cast<g2o::VertexSE3Expmap *>(optimizer.vertex(0));
        g2o::SE3Quat SE3quat_recov = p_SE3_recov->estimate();
        Mat pose = Converter::toCvMat(SE3quat_recov);
        p_frame->Set_pose(pose);

        return n_initial_correspondences - nbad;
    }

    void Optimizer::Local_bundadjustment(KeyFrame *p_keyframe, bool *pb_stop_flag, Map *p_map)
    {
        list<KeyFrame *> lp_local_keyframe;
        lp_local_keyframe.push_back(p_keyframe);

        p_keyframe->mn_mark_localba_id = p_keyframe->mn_keyframe_id;

        Map *p_cur_map = p_keyframe->Get_map();

        const vector<KeyFrame *> vp_neighbal_keyframe1 = p_keyframe->Get_all_covisibility_keyframe();
        for (auto p_keyframei : vp_neighbal_keyframe1)
        {
            p_keyframei->mn_mark_localba_id = p_keyframe->mn_keyframe_id;
            if (!p_keyframei->Is_bad() && p_keyframei->Get_map() == p_cur_map)
            {
                lp_local_keyframe.push_back(p_keyframei);
            }
        }

        list<MapPoint *> lp_local_mappoint;

        for (auto p_keyframei : lp_local_keyframe)
        {
            vector<MapPoint *> vp_local_mappointi = p_keyframei->Get_vect_mappoints();
            for (auto p_mpt : vp_local_mappointi)
            {
                if (p_mpt)
                {
                    if (!p_mpt->Is_bad() && p_mpt->Get_map() == p_cur_map)
                    {
                        if (p_mpt->mn_mark_localba_id != p_keyframe->mn_keyframe_id)
                        {
                            lp_local_mappoint.push_back(p_mpt);
                            p_mpt->mn_mark_localba_id = p_keyframe->mn_keyframe_id;
                        }
                    }
                }
            }
        }

        list<KeyFrame *> lp_fixed_keyframe;
        for (auto p_mpt : lp_local_mappoint)
        {
            map<KeyFrame *, int> obs = p_mpt->Get_observation();
            for (auto obsi : obs)
            {
                KeyFrame *p_keyframei = obsi.first;
                if (p_keyframei->mn_mark_localba_id != p_keyframe->mn_keyframe_id && p_keyframei->mn_mark_localba_fixed_id != p_keyframe->mn_keyframe_id)
                {
                    p_keyframei->mn_mark_localba_fixed_id = p_keyframe->mn_keyframe_id;
                    if (!p_keyframei->Is_bad() && p_keyframei->Get_map() == p_cur_map)
                    {
                        lp_fixed_keyframe.push_back(p_keyframei);
                    }
                }
            }
        }

        if (lp_fixed_keyframe.size() < 2)
        {
            int lower_id = p_keyframe->mn_keyframe_id;
            KeyFrame *p_lower_keyframe;
            int second_lower_id = p_keyframe->mn_keyframe_id;
            KeyFrame *p_second_lower_keyframe;

            for (auto p_keyframei : lp_local_keyframe)
            {
                if (p_keyframei == p_keyframe)
                {
                    continue;
                }
                if (p_keyframei->mn_keyframe_id < lower_id)
                {
                    second_lower_id = lower_id;
                    p_second_lower_keyframe = p_lower_keyframe;
                    lower_id = p_keyframei->mn_keyframe_id;
                    p_lower_keyframe = p_keyframei;
                }
                else if (p_keyframei->mn_keyframe_id < second_lower_id)
                {
                    second_lower_id = p_keyframei->mn_keyframe_id;
                    p_second_lower_keyframe = p_keyframei;
                }
            }

            lp_fixed_keyframe.push_back(p_lower_keyframe);
            lp_local_keyframe.remove(p_lower_keyframe);

            if (lp_fixed_keyframe.size() < 2)
            {
                lp_fixed_keyframe.push_back(p_second_lower_keyframe);
                lp_local_keyframe.remove(p_second_lower_keyframe);
            }
        }

        if (lp_fixed_keyframe.size() == 0)
        {
            cout << "fixed keyframe num: 0" << endl;
            return;
        }

        // cout<<"first fixed keyframe keyframe id:"<<lp_fixed_keyframe.front()->mn_KF_id<<endl;[]
        g2o::SparseOptimizer optimizer;
        string solver_name = "lm_fix6_3";
        g2o::OptimizationAlgorithmProperty solverProperty;
        optimizer.setAlgorithm(
            g2o::OptimizationAlgorithmFactory::instance()->construct(solver_name,
                                                                     solverProperty));
        optimizer.setVerbose(false);

        if (pb_stop_flag)
        {
            optimizer.setForceStopFlag(pb_stop_flag);
        }

        unsigned long max_keyframe_id = 0;

        for (auto p_keyframei : lp_local_keyframe)
        {
            g2o::VertexSE3Expmap *p_SE3_vertex = new g2o::VertexSE3Expmap();
            p_SE3_vertex->setEstimate(Converter::toSE3Quat(p_keyframei->Get_Tcw()));
            p_SE3_vertex->setId(p_keyframei->mn_keyframe_id);
            p_SE3_vertex->setFixed(false);
            optimizer.addVertex(p_SE3_vertex);
            if (p_keyframei->mn_keyframe_id > max_keyframe_id)
            {
                max_keyframe_id = p_keyframei->mn_keyframe_id;
            }
        }

        for (auto p_keyframei : lp_fixed_keyframe)
        {
            g2o::VertexSE3Expmap *p_SE3_vertex = new g2o::VertexSE3Expmap();
            p_SE3_vertex->setEstimate(Converter::toSE3Quat(p_keyframei->Get_Tcw()));
            p_SE3_vertex->setId(p_keyframei->mn_keyframe_id);
            p_SE3_vertex->setFixed(true);
            optimizer.addVertex(p_SE3_vertex);
            if (p_keyframei->mn_keyframe_id > max_keyframe_id)
            {
                max_keyframe_id = p_keyframei->mn_keyframe_id;
            }
        }

        const int n_expected_size = (lp_local_keyframe.size() + lp_fixed_keyframe.size()) * lp_local_mappoint.size();

        vector<g2o::EdgeSE3ProjectXYZ *> vp_edge_mono;
        vp_edge_mono.reserve(n_expected_size);
        vector<KeyFrame *> vp_edgeKF_mono;
        vp_edgeKF_mono.reserve(n_expected_size);
        vector<MapPoint *> vp_edgepoint_mono;
        vp_edgepoint_mono.reserve(n_expected_size);

        const float th2d = sqrt(5.991);

        int n_points = 0;

        int n_keyframe = lp_fixed_keyframe.size() + lp_local_keyframe.size();
        int n_edge = 0;

        for (auto p_mpt : lp_local_mappoint)
        {
            g2o::VertexPointXYZ *pPt = new g2o::VertexPointXYZ();
            pPt->setEstimate(Converter::toVector3d(p_mpt->Get_world_pose()));
            int id = p_mpt->mn_id + max_keyframe_id + 1;
            pPt->setId(id);
            pPt->setMarginalized(true);
            optimizer.addVertex(pPt);
            n_points++;

            const map<KeyFrame *, int> obs = p_mpt->Get_observation();

            for (auto obsi : obs)
            {
                KeyFrame *p_keyframei = obsi.first;
                if (!p_keyframei->Is_bad() && p_keyframei->Get_map() == p_cur_map)
                {
                    const int idx = obsi.second;

                    const KeyPoint &kpun = p_keyframei->mv_orb_unkeypoints[idx];
                    Eigen::Matrix<double, 2, 1> obs;
                    obs << kpun.pt.x, kpun.pt.y;
                    g2o::EdgeSE3ProjectXYZ *e = new g2o::EdgeSE3ProjectXYZ();
                    e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(id)));
                    e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(p_keyframei->mn_keyframe_id)));
                    e->setMeasurement(obs);
                    const float &invsigma2 = p_keyframei->mvd_inv_level_sigma2[kpun.octave];
                    e->setInformation(Eigen::Matrix2d::Identity() * invsigma2);

                    g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;
                    e->setRobustKernel(rk);
                    rk->setDelta(th2d);

                    // TODO
                    e->fx = 458.654;
                    e->fy = 457.296;
                    e->cx = 367.215;
                    e->cy = 248.375;

                    optimizer.addEdge(e);
                    vp_edge_mono.push_back(e);
                    vp_edgeKF_mono.push_back(p_keyframei);
                    vp_edgepoint_mono.push_back(p_mpt);
                    n_edge++;
                }
            }
        }

        if (pb_stop_flag)
        {
            if (*pb_stop_flag)
            {
                return;
            }
        }

        optimizer.initializeOptimization();
        optimizer.optimize(5);

        bool b_domore = true;

        if (pb_stop_flag)
        {
            if (*pb_stop_flag)
            {
                b_domore = false;
            }
        }

        if (b_domore)
        {
            int n_bad_obs = 0;
            for (int i = 0; i < vp_edge_mono.size(); i++)
            {
                g2o::EdgeSE3ProjectXYZ *e = vp_edge_mono[i];
                MapPoint *p_mpt = vp_edgepoint_mono[i];
                if (p_mpt->Is_bad())
                {
                    continue;
                }
                if (e->chi2() > 5.991 || !e->isDepthPositive())
                {
                    n_bad_obs++;
                }
            }
            optimizer.initializeOptimization(0);
            optimizer.optimize(10);
        }

        vector<pair<KeyFrame *, MapPoint *>> vpair_to_erase;
        vpair_to_erase.reserve(vp_edge_mono.size());

        for (int i = 0; i < vp_edge_mono.size(); i++)
        {
            g2o::EdgeSE3ProjectXYZ *e = vp_edge_mono[i];
            MapPoint *p_mpt = vp_edgepoint_mono[i];
            if (p_mpt->Is_bad())
            {
                continue;
            }
            if (e->chi2() > 5.991 || !e->isDepthPositive())
            {
                KeyFrame *p_keyframe_tmp = vp_edgeKF_mono[i];
                vpair_to_erase.push_back(make_pair(p_keyframe_tmp, p_mpt));
            }
        }

        unique_lock<mutex> lock(p_map->mmutex_mapupdate);
        if (!vpair_to_erase.empty())
        {
            for (auto p : vpair_to_erase)
            {
                KeyFrame *p_keyframei = p.first;
                MapPoint *p_mpt = p.second;
                p_keyframei->Erase_mappoint_match(p_mpt);
                p_mpt->Erase_observation(p_keyframei);
            }
        }

        for (auto p_keyframei : lp_local_keyframe)
        {
            g2o::VertexSE3Expmap *vSE3 = static_cast<g2o::VertexSE3Expmap *>(optimizer.vertex(p_keyframei->mn_keyframe_id));
            g2o::SE3Quat SE3quat = vSE3->estimate();
            p_keyframei->Set_pose(Converter::toCvMat(SE3quat));
        }

        for (auto p_mpt : lp_local_mappoint)
        {
            g2o::VertexPointXYZ *vpt = static_cast<g2o::VertexPointXYZ *>(optimizer.vertex(max_keyframe_id + 1 + p_mpt->mn_id));

            p_mpt->Set_world_pose(Converter::toCvMat(vpt->estimate()));
            p_mpt->Update_normal_and_depth();
        }

        p_map->Increase_change_index();
    }

    int Optimizer::Optimize_sim3(KeyFrame *p_keyframe1, KeyFrame *p_keyframe2, vector<MapPoint *> &vp_matches1, g2o::Sim3 &g2oS12, const float th2, const bool b_fix_scale, Eigen::Matrix<double, 7, 7> &mat_accum_hessian, const bool b_all_points)
    {
        g2o::SparseOptimizer optimizer;
        string solver_name = "lm_fix6_3";
        g2o::OptimizationAlgorithmProperty solverProperty;
        optimizer.setAlgorithm(
            g2o::OptimizationAlgorithmFactory::instance()->construct(solver_name,
                                                                     solverProperty));
        optimizer.setVerbose(false);

        const Mat R1w = p_keyframe1->Get_Rcw();
        const Mat t1w = p_keyframe1->Get_tcw();
        const Mat R2w = p_keyframe2->Get_Rcw();
        const Mat t2w = p_keyframe2->Get_tcw();

        g2o::VertexSim3Expmap *pv_sim3 = new g2o::VertexSim3Expmap();
        pv_sim3->_fix_scale = b_fix_scale;
        pv_sim3->setEstimate(g2oS12);
        pv_sim3->setId(0);
        pv_sim3->setFixed(false);

        float cx = 367.215;
        float cy = 248.375;
        float fx = 458.654;
        float fy = 457.296;
        // TODO
        pv_sim3->_principle_point1[0] = cx;
        pv_sim3->_principle_point1[1] = cy;
        pv_sim3->_focal_length1[0] = fx;
        pv_sim3->_focal_length1[1] = fy;
        pv_sim3->_principle_point2[0] = cx;
        pv_sim3->_principle_point2[1] = cy;
        pv_sim3->_focal_length2[0] = fx;
        pv_sim3->_focal_length2[1] = fy;

        optimizer.addVertex(pv_sim3);

        const int n_mpt_num = vp_matches1.size();
        const vector<MapPoint *> vp_mpt1 = p_keyframe1->Get_vect_mappoints();
        vector<g2o::EdgeSim3ProjectXYZ *> vp_edge12;
        vector<g2o::EdgeInverseSim3ProjectXYZ *> vp_edge21;
        vector<int> vn_edge_index;
        vector<bool> vb_in_keyframe2;

        vn_edge_index.reserve(2 * n_mpt_num);
        vp_edge12.reserve(2 * n_mpt_num);
        vp_edge21.reserve(2 * n_mpt_num);
        vb_in_keyframe2.reserve(2 * n_mpt_num);

        const float delta_huber = sqrt(th2);

        int n_correspondence = 0;
        vector<int> vn_id_only_in_keyframe2;

        for (int i = 0; i < n_mpt_num; i++)
        {
            if (!vp_matches1[i])
            {
                continue;
            }
            MapPoint *p_mpt1 = vp_mpt1[i];
            MapPoint *p_mpt2 = vp_matches1[i];

            const int id1 = 2 * i + 1;
            const int id2 = 2 * (i + i);

            const int i2 = p_mpt2->Get_index_in_keyframe(p_keyframe2);

            Mat p3dc1;
            Mat p3dc2;
            if (p_mpt1 && p_mpt2)
            {
                if (!p_mpt1->Is_bad() && !p_mpt2->Is_bad())
                {
                    g2o::VertexPointXYZ *pv_point1 = new g2o::VertexPointXYZ();
                    Mat p3dw1 = p_mpt1->Get_world_pose();
                    p3dc1 = R1w * p3dw1 + t1w;
                    pv_point1->setEstimate(Converter::toVector3d(p3dc1));
                    pv_point1->setId(id1);
                    pv_point1->setFixed(true);
                    optimizer.addVertex(pv_point1);

                    g2o::VertexPointXYZ *pv_point2 = new g2o::VertexPointXYZ();
                    Mat p3dw2 = p_mpt2->Get_world_pose();
                    p3dc2 = R2w * p3dw2 + t2w;
                    pv_point2->setEstimate(Converter::toVector3d(p3dc2));
                    pv_point2->setId(id2);
                    pv_point2->setFixed(true);
                    optimizer.addVertex(pv_point2);
                }
                else
                {
                    continue;
                }
            }
            else if (p_mpt2)
            {
                if (!p_mpt2->Is_bad())
                {
                    g2o::VertexPointXYZ *pv_point2 = new g2o::VertexPointXYZ();
                    Mat p3dw2 = p_mpt2->Get_world_pose();
                    p3dc2 = R2w * p3dw2 + t2w;
                    pv_point2->setEstimate(Converter::toVector3d(p3dc2));
                    pv_point2->setId(id2);
                    pv_point2->setFixed(true);
                    optimizer.addVertex(pv_point2);
                    vn_id_only_in_keyframe2.push_back(id2);
                }
                continue;
            }
            else
            {
                continue;
            }
            if (i2 < 0 && !b_all_points)
            {
                continue;
            }
            if (p3dc2.at<float>(2) < 0)
            {
                continue;
            }
            n_correspondence++;
            Eigen::Matrix<double, 2, 1> obs1;
            const KeyPoint &unkp1 = p_keyframe1->mv_orb_unkeypoints[i];
            obs1 << unkp1.pt.x, unkp1.pt.y;

            g2o::EdgeSim3ProjectXYZ *e12 = new g2o::EdgeSim3ProjectXYZ();
            e12->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(id2)));
            e12->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(0)));
            e12->setMeasurement(obs1);

            const float &inv_sigma_square1 = p_keyframe1->mvd_inv_level_sigma2[unkp1.octave];
            e12->setInformation(Eigen::Matrix2d::Identity() * inv_sigma_square1);
            g2o::RobustKernelHuber *rk1 = new g2o::RobustKernelHuber;
            e12->setRobustKernel(rk1);
            rk1->setDelta(delta_huber);
            optimizer.addEdge(e12);

            Eigen::Matrix<double, 2, 1> obs2;
            KeyPoint unkp2;
            bool b_in_keyframe2;
            if (i2 >= 0)
            {
                unkp2 = p_keyframe2->mv_orb_unkeypoints[i2];
                obs2 << unkp2.pt.x, unkp2.pt.y;
                b_in_keyframe2 = true;
            }
            else
            {
                float invz = 1 / p3dc2.at<float>(2);
                float x = p3dc2.at<float>(0);
                float y = p3dc2.at<float>(1);
                obs2 << x, y;
                unkp2 = KeyPoint(Point2f(2, y), p_mpt2->mn_tarcked_level);
                b_in_keyframe2 = false;
            }

            g2o::EdgeInverseSim3ProjectXYZ *e21 = new g2o::EdgeInverseSim3ProjectXYZ();
            e21->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(id1)));
            e21->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(0)));
            e21->setMeasurement(obs2);
            const float &inv_sigma_square2 = p_keyframe2->mvd_inv_level_sigma2[unkp2.octave];
            e21->setInformation(Eigen::Matrix2d::Identity() * inv_sigma_square2);
            g2o::RobustKernelHuber *rk2 = new g2o::RobustKernelHuber;
            e21->setRobustKernel(rk2);
            rk2->setDelta(delta_huber);
            optimizer.addEdge(e21);

            vp_edge12.push_back(e12);
            vp_edge21.push_back(e21);
            vn_edge_index.push_back(i);
            vb_in_keyframe2.push_back(b_in_keyframe2);
        }

        optimizer.initializeOptimization();
        optimizer.optimize(5);

        int n_bad = 0;
        for (int i = 0; i < vp_edge12.size(); i++)
        {
            g2o::EdgeSim3ProjectXYZ *e12 = vp_edge12[i];
            g2o::EdgeInverseSim3ProjectXYZ *e21 = vp_edge21[i];
            if (!e12 || !e21)
            {
                continue;
            }
            if (e12->chi2() > th2 || e21->chi2() > th2)
            {
                int idx = vn_edge_index[i];
                vp_matches1[idx] = static_cast<MapPoint *>(NULL);
                optimizer.removeEdge(e12);
                optimizer.removeEdge(e21);
                vp_edge12[i] = static_cast<g2o::EdgeSim3ProjectXYZ *>(NULL);
                vp_edge21[i] = static_cast<g2o::EdgeInverseSim3ProjectXYZ *>(NULL);
                n_bad++;

                continue;
            }

            e12->setRobustKernel(0);
            e21->setRobustKernel(0);
        }

        int n_more_iterations;
        if (n_bad > 0)
        {
            n_more_iterations = 10;
        }
        else
        {
            n_more_iterations = 5;
        }
        if (n_correspondence - n_bad < 10)
        {
            return 0;
        }

        optimizer.initializeOptimization();
        optimizer.optimize(n_more_iterations);

        int n_in = 0;
        mat_accum_hessian = Eigen::MatrixXd::Zero(7, 7);
        for (int i = 0; i < vp_edge12.size(); i++)
        {
            g2o::EdgeSim3ProjectXYZ *e12 = vp_edge12[i];
            g2o::EdgeInverseSim3ProjectXYZ *e21 = vp_edge21[i];
            if (!e12 || !e21)
            {
                continue;
            }
            e12->computeError();
            e21->computeError();
            if (e12->chi2() > th2 || e21->chi2() > th2)
            {
                int idx = vn_edge_index[i];
                vp_matches1[idx] = static_cast<MapPoint *>(NULL);
            }
            else
            {
                n_in++;
            }
        }

        g2o::VertexSim3Expmap *pv_sim3_recov = static_cast<g2o::VertexSim3Expmap *>(optimizer.vertex(0));
        g2oS12 = pv_sim3_recov->estimate();

        return n_in;
    }

    void Optimizer::Optimize_essential_graph(Map *p_map, KeyFrame *p_loop_keyframe, KeyFrame *p_cur_keyframe, const type_keyframe_and_pose &non_corrected_sim3, const type_keyframe_and_pose &corrected_sim3, const map<KeyFrame *, set<KeyFrame *>> &map_loop_connection, const bool &b_fix_scale)
    {
        g2o::SparseOptimizer optimizer;
        string solver_name = "lm_fix7_3";
        g2o::OptimizationAlgorithmProperty solverProperty;

        optimizer.setAlgorithm(
            g2o::OptimizationAlgorithmFactory::instance()->construct(solver_name,
                                                                     solverProperty));

        optimizer.setVerbose(false);

        const vector<KeyFrame *> vp_all_keyframe = p_map->Get_all_keyframe();
        const vector<MapPoint *> vp_all_mpt = p_map->Get_all_mappoint();

        const unsigned int n_max_id = p_map->Get_max_keyframeid();

        vector<g2o::Sim3, Eigen::aligned_allocator<g2o::Sim3>> vsim3_Scw(n_max_id + 1);
        vector<g2o::Sim3, Eigen::aligned_allocator<g2o::Sim3>> vsim3_Swc_corrected(n_max_id + 1);

        const int n_weight_th = 100;

        for (int i = 0; i < vp_all_keyframe.size(); i++)
        {
            KeyFrame *p_keyframei = vp_all_keyframe[i];
            if (p_keyframei->Is_bad())
            {
                continue;
            }
            g2o::VertexSim3Expmap *pv_sim3 = new g2o::VertexSim3Expmap();
            const int n_id = p_keyframei->mn_keyframe_id;

            type_keyframe_and_pose::const_iterator it = corrected_sim3.find(p_keyframei);
            if (it != corrected_sim3.end())
            {
                vsim3_Scw[n_id] = it->second;
                pv_sim3->setEstimate(it->second);
            }
            else
            {
                Eigen::Matrix<double, 3, 3> Rcw = Converter::toMatrix3d(p_keyframei->Get_Rcw());
                Eigen::Matrix<double, 3, 1> tcw = Converter::toVector3d(p_keyframei->Get_tcw());
                g2o::Sim3 sim3_Siw(Rcw, tcw, 1.0);
                vsim3_Scw[n_id] = sim3_Siw;
                pv_sim3->setEstimate(sim3_Siw);
            }
            if (p_keyframei->mn_keyframe_id == p_map->Get_init_KFid())
            {
                pv_sim3->setFixed(true);
            }
            pv_sim3->setId(n_id);
            pv_sim3->setMarginalized(false);
            pv_sim3->_fix_scale = b_fix_scale;
            optimizer.addVertex(pv_sim3);
        }

        set<pair<long unsigned int, long unsigned int>> s_inserted_edge;
        const Eigen::Matrix<double, 7, 7> matlambda = Eigen::Matrix<double, 7, 7>::Identity();

        int n_loop = 0;
        for (auto item : map_loop_connection)
        {
            KeyFrame *p_keyframei = item.first;
            const long unsigned int n_id_i = p_keyframei->mn_keyframe_id;
            const set<KeyFrame *> sp_connection = item.second;
            const g2o::Sim3 sim3_Siw = vsim3_Scw[n_id_i];
            const g2o::Sim3 sim3_Swi = sim3_Siw.inverse();

            for (auto p_keyframej : sp_connection)
            {
                const long unsigned int n_id_j = p_keyframej->mn_keyframe_id;
                if ((n_id_j != p_cur_keyframe->mn_keyframe_id || n_id_j != p_loop_keyframe->mn_keyframe_id) && p_keyframei->Get_weight(p_keyframej) < n_weight_th)
                {
                    continue;
                }
                const g2o::Sim3 sim3_Sjw = vsim3_Scw[n_id_j];
                const g2o::Sim3 sim3_Sji = sim3_Sjw * sim3_Swi;

                g2o::EdgeSim3 *e = new g2o::EdgeSim3();
                e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(n_id_j)));
                e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(n_id_i)));
                e->setMeasurement(sim3_Sji);
                e->information() = matlambda;
                optimizer.addEdge(e);
                n_loop++;
                s_inserted_edge.insert(make_pair(min(n_id_i, n_id_j), max(n_id_i, n_id_j)));
            }
        }

        int n_spa_tree = 0;
        int n_cov = 0;
        int n_kf = 0;
        for (int i = 0; i < vp_all_keyframe.size(); i++)
        {
            KeyFrame *p_keyframei = vp_all_keyframe[i];
            const int n_id_i = p_keyframei->mn_keyframe_id;
            g2o::Sim3 sim3_Swi;
            type_keyframe_and_pose::const_iterator iti = non_corrected_sim3.find(p_keyframei);
            if (iti != non_corrected_sim3.end())
            {
                sim3_Swi = (iti->second).inverse();
            }
            else
            {
                sim3_Swi = vsim3_Scw[n_id_i].inverse();
            }
            KeyFrame *p_keyframei_parent = p_keyframei->Get_parent();

            if (p_keyframei_parent)
            {
                int n_id_j = p_keyframei_parent->mn_keyframe_id;
                g2o::Sim3 sim3_Sjw;
                type_keyframe_and_pose::const_iterator itj = non_corrected_sim3.find(p_keyframei_parent);
                if (itj != non_corrected_sim3.end())
                {
                    sim3_Sjw = itj->second;
                }
                else
                {
                    sim3_Sjw = vsim3_Scw[n_id_j];
                }

                g2o::Sim3 sim3_Sji = sim3_Sjw * sim3_Swi;
                g2o::EdgeSim3 *e = new g2o::EdgeSim3();
                e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(n_id_j)));
                e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(n_id_i)));
                e->setMeasurement(sim3_Sji);
                n_kf++;
                n_spa_tree++;
                e->information() = matlambda;
                optimizer.addEdge(e);
            }

            const set<KeyFrame *> sp_loop_edge = p_keyframei->Get_loop_edge();
            for (auto p_keyframej : sp_loop_edge)
            {
                if (p_keyframej->mn_keyframe_id < p_keyframei->mn_keyframe_id)
                {
                    g2o::Sim3 sim3_Sjw;
                    type_keyframe_and_pose::const_iterator itj = non_corrected_sim3.find(p_keyframej);
                    if (itj != non_corrected_sim3.end())
                    {
                        sim3_Sjw = itj->second;
                    }
                    else
                    {
                        sim3_Sjw = vsim3_Scw[p_keyframej->mn_keyframe_id];
                    }
                    g2o::Sim3 sim3_Sji = sim3_Sjw * sim3_Swi;
                    g2o::EdgeSim3 *e = new g2o::EdgeSim3();
                    e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(p_keyframej->mn_keyframe_id)));
                    e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(n_id_i)));
                    e->setMeasurement(sim3_Sji);
                    e->information() = matlambda;
                    optimizer.addEdge(e);
                    n_kf++;
                    n_loop++;
                }
            }
            const vector<KeyFrame *> vp_connect_keyframe = p_keyframei->Get_covisible_by_weight(n_weight_th);
            for (auto p_keyframej : vp_connect_keyframe)
            {
                if (p_keyframej && p_keyframej != p_keyframei_parent && !p_keyframei->Has_children(p_keyframej) && !sp_loop_edge.count(p_keyframej))
                {
                    if (s_inserted_edge.count(make_pair(min(p_keyframei->mn_keyframe_id, p_keyframej->mn_keyframe_id), max(p_keyframei->mn_keyframe_id, p_keyframej->mn_keyframe_id))))
                    {
                        continue;
                    }
                    g2o::Sim3 sim3_Sjw;
                    type_keyframe_and_pose::const_iterator itj = non_corrected_sim3.find(p_keyframej);
                    if (itj != non_corrected_sim3.end())
                    {
                        sim3_Sjw = itj->second;
                    }
                    else
                    {
                        sim3_Sjw = vsim3_Scw[p_keyframej->mn_keyframe_id];
                    }
                    g2o::Sim3 sim3_Sji = sim3_Sjw * sim3_Swi;
                    g2o::EdgeSim3 *e = new g2o::EdgeSim3();
                    e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(p_keyframej->mn_keyframe_id)));
                    e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(n_id_i)));
                    e->setMeasurement(sim3_Sji);
                    e->information() = matlambda;
                    optimizer.addEdge(e);
                    n_kf++;
                    n_cov++;
                }
            }
        }

        optimizer.initializeOptimization();
        optimizer.computeActiveErrors();
        float err0 = optimizer.activeRobustChi2();
        optimizer.optimize(20);
        optimizer.computeActiveErrors();
        float err_end = optimizer.activeRobustChi2();
        unique_lock<mutex> lock(p_map->mmutex_mapupdate);

        for(int i=0;i<vp_all_keyframe.size();i++){
            KeyFrame* p_keyframei = vp_all_keyframe[i];
            const int n_id_i = p_keyframei->mn_keyframe_id;
            g2o::VertexSim3Expmap *pv_sim3 = static_cast<g2o::VertexSim3Expmap*>(optimizer.vertex(n_id_i));
            g2o::Sim3 sim3_Siw_corrected = pv_sim3->estimate();
            vsim3_Swc_corrected[n_id_i] = sim3_Siw_corrected.inverse();
            Eigen::Matrix3d eig_R = sim3_Siw_corrected.rotation().toRotationMatrix();
            Eigen::Vector3d eig_t = sim3_Siw_corrected.translation();
            double s = sim3_Siw_corrected.scale();
            eig_t *=(1./s);
            Mat mat_Tiw = Converter::toCvSE3(eig_R,eig_t);
            p_keyframei->Set_pose(mat_Tiw);
        }

        for(int i=0;i<vp_all_mpt.size();i++){
            MapPoint* p_mpti = vp_all_mpt[i];
            if(p_mpti->Is_bad()){
                continue;
            }
            int n_id_r;
            if(p_mpti->mn_mark_corrected_by_keyframe==p_cur_keyframe->mn_keyframe_id){
                n_id_r = p_mpti->mn_mark_corrected_refkeyframe;
            }else{
                KeyFrame * p_keyframe_rf = p_mpti->Get_reference_keyframe();
                n_id_r = p_keyframe_rf->mn_keyframe_id;
            }
            g2o::Sim3 sim3_Srw = vsim3_Scw[n_id_r];
            g2o::Sim3 sim3_Swr_corrected = vsim3_Swc_corrected[n_id_r];
            Mat mat_p3dw = p_mpti->Get_world_pose();
            Eigen::Matrix<double,3,1> eig_p3dw = Converter::toVector3d(mat_p3dw);
            Eigen::Matrix<double,3,1> eig_p3dw_corrected = sim3_Swr_corrected.map(sim3_Srw.map(eig_p3dw));
            Mat mat_p3dw_corrected = Converter::toCvMat(eig_p3dw_corrected);
            p_mpti->Set_world_pose(mat_p3dw_corrected);
            p_mpti->Update_normal_and_depth();
        }
        p_map->Increase_change_index();
    }

}